#include <stdlib.h>
#include <stdio.h>
#include <stdarg.h>
#include <time.h>
#include <math.h>

#include "pid.h"

#define SAMPLE_TIME 0.01f
#define TIME_LIMIT 10.01f

#define TARGET_VALUE 100.0f

const PidConfigType pid_cfg = {
    50.0f,
    3.0f,
    0.0f,
    -5000.0f,
    5000.0f,
};

FILE *log_fp = NULL;

PidCompuateType rand_float(PidCompuateType x, PidCompuateType y);
PidCompuateType System(PidCompuateType output);
void log_printf(const char *fmt, ...);

int main()
{
    srand(time(NULL));
    log_fp = fopen("build/latest.log", "w");

    PidCompuateType target = TARGET_VALUE;
    PidCompuateType output = 0.0f;
    PidCompuateType mesurement = 0.0f;
    PidCompuateType error = target;
    PidCompuateType time = 0.0f;

    PidCompuateType ts = 0.0f;
    PidCompuateType over_shoot = 0.0f;

    PidHandler pid;

    PidConfig(&pid, &pid_cfg);
    PidReset(&pid);

    log_printf("PID started\n");
    while (time < TIME_LIMIT)
    {
        log_printf("Time:%5.2fs Output:%7.3f Mesurement:%7.3f Error:%7.3f\n", time, output, mesurement, error);

        output = PidCompute(&pid, error);

        mesurement = System(output);
        error = target - mesurement;
        time += SAMPLE_TIME;
        if (fabs(error) > target * 0.02f)
        {
            ts = time;
        }
        over_shoot = fmax(over_shoot, mesurement);
    }
    log_printf("Error:%.3f\n", error);
    log_printf("Ts(2%%):%.2fs\n", ts);
    log_printf("Over shoot:%.3f\n", over_shoot);

    fclose(log_fp);

    return 0;
}

PidCompuateType rand_float(PidCompuateType x, PidCompuateType y)
{
    return x + 1.0 * rand() / RAND_MAX * (y - x);
}

// 系统是 w(s)/I(s)=Kt/(Js+F)
// ω(s) 表示转速的拉普拉斯变换，Kt​ 是电机转矩常数，J 是转动惯量，F 是粘滞摩擦系数，I(s) 是电枢电流的拉普拉斯变换
const PidCompuateType Kt = 3.0f;
const PidCompuateType J = 5.0f;
const PidCompuateType F = 1.0f;
const PidCompuateType disturb_max = 0.5f;
const PidCompuateType disturb_min = -0.5f;
PidCompuateType System(PidCompuateType input)
{
    static PidCompuateType speed = 0.0f;

    // speed += SAMPLE_TIME * (Kt / J * input - F / J * speed);
    speed += SAMPLE_TIME * (Kt / J * input - (F + rand_float(disturb_min, disturb_max)) / J * speed);

    return speed;
}

void log_printf(const char *fmt, ...)
{
    va_list args;
    va_start(args, fmt);
    vfprintf(log_fp, fmt, args);
    vprintf(fmt, args);
    va_end(args);
}
